#include "Phycccp.h"
#include "../shared/Time.h"
#include "../utils/Profiler.h"

namespace Putin{
PhysicsHandler::PhysicsHandler(WorldLink* worldlink)
{
	//This is used for world height querries
	mWorldLink = worldlink;

	//Holds pointers to geometry and performs broad phase collision detection
	mCollisionDetection = new CollisionDetection();
	mCollisionsOn = false;
}
PhysicsHandler::~PhysicsHandler(void)
{
delete(mCollisionDetection);
delete(mWorldLink);
} //do the maps have to be deleted?

void PhysicsHandler::AddDynamicObject(Unit * unit,int unit_nr)
{
	//Creates a new game object
	DynamicPhysicsObject* dobject = new DynamicPhysicsObject(unit,mWorldLink);

	//Add unit to the unitmap
	mDynamicObjectMap.insert(std::make_pair(unit_nr,dobject));

	//Add unit to collision detector
	mCollisionDetection->AddGeometry(dobject->mGeometry);
}
void PhysicsHandler::RemoveDynamicObject(int unit_nr)
{

	std::map<int, DynamicPhysicsObject*>::iterator it = mDynamicObjectMap.find(unit_nr);
	if( it != mDynamicObjectMap.end())
	{
		//Removes the unit from SAP
		it->second->mGeometry->mIsDesposed = true;
		mCollisionDetection->RemoveDisposed();
		//Delete unit
		delete it->second;
		//Remove unit from map
		mDynamicObjectMap.erase(it);
	}
}

void PhysicsHandler::AddStaticObject(GameObject* gameobject, int obj_nr)
{
	//Should add the passing of constant conditions
	StaticPhysicsObject* sobject = new StaticPhysicsObject(gameobject);

	//Add object to static object map
	mStaticObjectMap.insert(std::make_pair(obj_nr,sobject));

	//Add unit to collision detector
	mCollisionDetection->AddGeometry(sobject->mGeometry);
}
void PhysicsHandler::RemoveStaticObject(int obj_nr)
{
	std::map<int, StaticPhysicsObject*>::iterator it = mStaticObjectMap.find(obj_nr);
	if( it != mStaticObjectMap.end())
	{
		//Removes the unit from SAP
		it->second->mGeometry->mIsDesposed = true;
		mCollisionDetection->RemoveDisposed();
		//Delete unit
		delete it->second;
		//Remove unit from map
		mStaticObjectMap.erase(it);
	}
}

void PhysicsHandler::AddProjectileObject(Bullet* bullet)
{
	//Creates a new game object
	ProjectilePhysicsObject* pobject = new ProjectilePhysicsObject(bullet, mCollisionDetection);

	//Add object to static object map
	mProjectileObjectList.push_back(pobject);
}


void PhysicsHandler::Update()
{
	//Pre collision unit update
	for each(std::pair<int, DynamicPhysicsObject*> c in mDynamicObjectMap)
	{
		c.second->UpdatePrimary();
	}

	//Update projectiles


	for each(ProjectilePhysicsObject* c in mProjectileObjectList)
	{
		c->Update();
	}
	
		

	//Check for collisions
	
		startProfile("mCollisionDetection->run");
		mCollisionDetection->Run(mCollisionsOn);
		stopProfile("mCollisionDetection->run");


	mCollisionDetection->RemoveDisposed();
	std::list<ProjectilePhysicsObject*>::iterator itr = mProjectileObjectList.begin();
	for(;itr != mProjectileObjectList.end();)
	{
		ProjectilePhysicsObject* c = *itr;
		if( c->mIsDesposed )
		{
			mProjectileObjectList.erase(itr++);
			delete(c);
		}
		else
		{
			++itr;
		}

	}
	
	//Do post collision update
	for each(std::pair<int, DynamicPhysicsObject*> c in mDynamicObjectMap)
	{
		c.second->UpdateSecondary();
	}	

}

DynamicPhysicsObject::DynamicPhysicsObject(Unit * unit, WorldLink* worldlink)
{
	mType = DynamicType;
	mOffset = 0;
	//Store a UnitConstants pointer object
	mConstants = unit->mConstants;
	mUnit = unit;

	//Object called for height checks and OBB/AABB data
	mWorldLink = worldlink;

	//Starting values
	mWaypoint = mPosition = mUnit->getPosition();
	mPosition.z = mWorldLink->getHeight(mPosition)+mOffset;
	mSpeed = Vec3f(0,0,0);
	mUnit->setPosition(mPosition);
	mDirection = mUnit->getFacingDirection(); 
	mTorque = 0;



	//Setup the geometry based on data in pointer to unit
	CreateGeometry();	

}
DynamicPhysicsObject::~DynamicPhysicsObject()
{
	delete(mGeometry);
}

void DynamicPhysicsObject::UpdatePrimary()
{
	mPosition = mUnit->getPosition();
	Vec3f mDirection = mUnit->getFacingDirection();
	
	//mDirection.x =  direction.x; 
	//mDirection.y = direction.y;
	mWaypoint = mUnit->getNextWaypoint();

	/* Update control law, as is this has to be done every 
	 * frame but could be rewritten to every other frame
	 */
	if( mUnit->getState() != Unit::MOVING )
		mTorque = 0;
	else
		UpdateControl();

	//Time since last frame
	float h = Time::instance().getLastFrameTime();

	//add gravity term! could probably base this on the previous gradient (mNormal)
	//maybe as easy as a multiplier added to mSpeed

	//Update physics
	mSpeed = mSpeed*(1-mConstants->tracktion*h) + mDirection*mTorque*h;
	
	mPositionOld = mPosition;
	mPosition = mPosition + mSpeed*h;

	//Check so that unit is clamped to the ground;
	mPosition.z = mWorldLink->getHeight(mPosition)+mOffset;

	/*
	 * Update based on new position and normal, 
	 * this updates position in geometry and thus the position in SAP
	 */
	mNormal = mWorldLink->getNormal(mPosition);
	mGeometry->UpdateAABB(mPosition);	
}

void DynamicPhysicsObject::UpdateSecondary()
{
	/*
	 * The position of the mAABB is assumed to be the units position.
	 * If this turns out to cause problems, just add a position vector to geometry and use that
	 */
	float h = Time::instance().getLastFrameTime();
	
	//mSpeed = (mGeometry->mAABB.P-mPositionOld)/h;
	
	//check if position was corrected
	/*if (mPosition != mGeometry->mAABB.P)
	{
		mNormal = mWorldLink->getNormal(mGeometry->mAABB.P);
		mPosition = mGeometry->mAABB.P;
	}*/

	mUnit->setPosition(mPosition); 
	mUnit->setUpVector(mNormal);
	mUnit->setFacingDirection(Vec3f(mDirection.x,mDirection.y,0));
	
}

void DynamicPhysicsObject::UpdateControl()
{
	//Time since last frame
	float h = Time::instance().getLastFrameTime();

	//Caluclate the angle between reference and direction in x/y-plane 
    Vec2f targetVector;
	targetVector.x = mWaypoint.x-mPosition.x;
	targetVector.y = mWaypoint.y-mPosition.y;

    float perpDot = mDirection.x * targetVector.y - mDirection.y * targetVector.x;
	float normalDot = mDirection.x * targetVector.x + mDirection.y * targetVector.y;

	float alphaDiff = atan2(perpDot, normalDot);	//angle between reference and direction
	
	//Calculate how much the unit should turn and update direction
	float alpha = std::min(abs(alphaDiff), mConstants->turningSmoothener)//clamp at turningSmoothener
					* (-1*(alphaDiff<0)+1*(alphaDiff>0))				//Sign
					/ mConstants->turningSmoothener						//normalize
					* mConstants->turningSpeed							//multiplier 
					* h;										//time since last frame
	mDirectionOld = mDirection;
	mDirection.x = cos(alpha) * mDirection.x - sin(alpha) * mDirection.y;
	mDirection.y = sin(alpha) * mDirection.x + cos(alpha) * mDirection.y;
	mDirection.z = 0;


	mDirection = mDirection.normalized();

	//First check if target is within fov then calculate torque
	float torqueModifier;
	float absAlphaDiff = fabs(alphaDiff);
	if (absAlphaDiff > M_PI * mConstants->fov){
        mTorque = 0;}
	else{
		if (mConstants->accelerationType){
			torqueModifier =  1 - mConstants->fov / M_PI * absAlphaDiff;
		}
		else{
			torqueModifier = 1;
		}
		mTorque = mTorque + (mConstants->maxTorque  - mTorque) * h / mConstants->acceleration;
	}	
}

void DynamicPhysicsObject::CreateGeometry()
{
	//Setup these...
	AxisAlignedBoundingBox AABB;
	OrientedBoundingBox OBB;
	
	//Uses a reference to the unit object to create a geometry with bounding boxes
	AABB.P = mPosition;
	OBB.P = mPosition;

	//setup .E and .X, .Z,.Y also
	//AABB.E = mWorldLink->getUnitAABBhalfSize(mUnit->getID());
	OBB.E = Vec3f(9,5,5);
	//OBB.E = mWorldLink->getUnitOBBhalfSize(mUnit->getID());
	AABB.E = Vec3f(11,11,3);
	//OBB.E = Vec3f(10,10,10);
	//Thes should be removed since they are always unity
	OBB.X = Vec3f(1,0,0);
	OBB.Y = Vec3f(0,1,0);
	OBB.Z = Vec3f(0,0,1);

	mGeometry = new Geometry(AABB,OBB,this);
}

StaticPhysicsObject::StaticPhysicsObject(GameObject* gameobject)
{
	mType = StaticType;
	//Setup geometry BBs based on data
	AxisAlignedBoundingBox AABB;
	OrientedBoundingBox OBB;
	
	//Uses a reference to the unit object to create a geometry with bounding boxes
	AABB.P = gameobject->getPosition();
	OBB.P = gameobject->getPosition();

	//setup .E and .X, .Z,.Y also
	AABB.E = Vec3f(3,3,3);
	OBB.E = Vec3f(3,3,3);
	OBB.X = Vec3f(1,0,0);
	OBB.Y = Vec3f(0,1,0);
	OBB.Z = Vec3f(0,0,1);

	mGeometry = new Geometry(AABB,OBB,this);
}
StaticPhysicsObject::~StaticPhysicsObject(){
	delete(mGeometry);
}

ProjectilePhysicsObject::ProjectilePhysicsObject(Bullet* bullet, CollisionDetection* cd)
{
	mIsDesposed = false;
	mType = ProjectileType;
	mCollisionDetection = cd;

	mBullet = bullet;
	mPoc = mBullet->mProjectileObjectConstants;
	mStartPosition = mBullet->getPosition();
	mTarget = mBullet->mTargetLoc;


	float d = sqrt((mStartPosition.x-mTarget.x)*(mStartPosition.x-mTarget.x)+(mStartPosition.y-mTarget.y)*(mStartPosition.y-mTarget.y));
	float a = tan((mTarget.z-mStartPosition.z)/d);
	if (mPoc->initialAngle < (M_PI/18 + a))  
		mInitialAngle = (M_PI/18 + a);
	else 
		mInitialAngle = mPoc->initialAngle;

	float abs_v = sqrt(-mPoc->gravity*d*d/(2*cos(mInitialAngle)*cos(mInitialAngle))/((mTarget.z-mStartPosition.z)-d*tan(mInitialAngle)));

	//mSpeed = Vec3f((target.x-position.x),(target.y-position.y),0).normalized()+Vec3f(0,0,1);
	//mSpeed = mSpeed|Vec3f(cos(poc->initialAngle),cos(poc->initialAngle),sin(poc->initialAngle));
	mDirection = Vec3f((mTarget.x-mStartPosition.x),(mTarget.y-mStartPosition.y),0).normalized()*cos(mInitialAngle)+Vec3f(0,0,sin(mInitialAngle));
	mDirection = mDirection*abs_v;
	t_final = d/abs_v/cos(mInitialAngle);
	t = 0;
}
ProjectilePhysicsObject::~ProjectilePhysicsObject(){}
void ProjectilePhysicsObject::Update(void)
{
	if(!mIsDesposed)
	{
		//Check for explosion
		if (t > t_final*mPoc->speed)
		{	
			AxisAlignedBoundingBox AABB;
			OrientedBoundingBox OBB;
			AABB.P = mTarget;
			AABB.E = Vec3f(1,1,1)*mPoc->splashRange;

			Geometry *geometry = new Geometry(AABB,OBB,this);
			mCollisionDetection->AddGeometry(geometry);
			geometry->mIsDesposed = true;
			mIsDesposed = true;
			t = t_final*mPoc->speed;
		}
		mPosition = mStartPosition + mDirection*t/mPoc->speed+Vec3f(0,0,1)*(-0.5*mPoc->gravity*t*t/mPoc->speed/mPoc->speed);
		this->mBullet->mPos = mPosition;
		this->mBullet->mFacingDirection = mDirection;
		t = t + Time::instance().getLastFrameTime();
	}
}



}//End of namespace putin